//
// File: calcAR.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 20-Mar-2025 09:03:00
//

// Include Files
#include "calcAR.h"
#include "calcAR_data.h"
#include "calcAR_initialize.h"
#include "calcAR_rtwutil.h"
#include "interp1.h"
#include "pburg.h"
#include "rt_nonfinite.h"
#include "coder_array.h"
#include <cmath>

// Function Declarations
static void times(coder::array<double, 1U> &in1,
                  const coder::array<double, 1U> &in2);

// Function Definitions
//
// Arguments    : coder::array<double, 1U> &in1
//                const coder::array<double, 1U> &in2
// Return Type  : void
//
static void times(coder::array<double, 1U> &in1,
                  const coder::array<double, 1U> &in2)
{
  coder::array<double, 1U> b_in1;
  int i;
  int loop_ub;
  int stride_0_0;
  int stride_1_0;
  if (in2.size(0) == 1) {
    i = in1.size(0);
  } else {
    i = in2.size(0);
  }
  b_in1.set_size(i);
  stride_0_0 = (in1.size(0) != 1);
  stride_1_0 = (in2.size(0) != 1);
  if (in2.size(0) == 1) {
    loop_ub = in1.size(0);
  } else {
    loop_ub = in2.size(0);
  }
  for (i = 0; i < loop_ub; i++) {
    b_in1[i] = in1[i * stride_0_0] * in2[i * stride_1_0];
  }
  in1.set_size(b_in1.size(0));
  loop_ub = b_in1.size(0);
  for (i = 0; i < loop_ub; i++) {
    in1[i] = b_in1[i];
  }
}

//
// calAR - Calculates the PSD using Auto Regression model.
//
// Inputs:
// Outputs:
//
// Arguments    : const coder::array<double, 2U> &t
//                const coder::array<double, 2U> &y
//                double fs
//                double nfft
//                double AR_order
//                coder::array<double, 1U> &PSD
//                coder::array<double, 1U> &F
// Return Type  : void
//
void calcAR(const coder::array<double, 2U> &t,
            const coder::array<double, 2U> &y, double fs, double nfft,
            double AR_order, coder::array<double, 1U> &PSD,
            coder::array<double, 1U> &F)
{
  coder::array<double, 2U> Vq;
  coder::array<double, 2U> b_y;
  coder::array<double, 2U> t2;
  coder::array<double, 2U> x;
  coder::array<double, 1U> c_y;
  coder::array<double, 1U> d_y;
  coder::array<double, 1U> e_y;
  double cdiff;
  double d;
  double ndbl;
  int hi;
  int ib;
  int k;
  int lastBlockLength;
  int nm1d2;
  int nx;
  signed char w_data;
  boolean_T b;
  if (!isInitialized_calcAR) {
    calcAR_initialize();
  }
  // Prepare y
  d = 1.0 / fs;
  if (std::isnan(t[0]) || std::isnan(d) || std::isnan(t[t.size(1) - 1])) {
    t2.set_size(1, 1);
    t2[0] = rtNaN;
  } else if ((d == 0.0) || ((t[0] < t[t.size(1) - 1]) && (d < 0.0)) ||
             ((t[t.size(1) - 1] < t[0]) && (d > 0.0))) {
    t2.set_size(1, 0);
  } else if ((std::isinf(t[0]) || std::isinf(t[t.size(1) - 1])) &&
             (std::isinf(d) || (t[0] == t[t.size(1) - 1]))) {
    t2.set_size(1, 1);
    t2[0] = rtNaN;
  } else if (std::isinf(d)) {
    t2.set_size(1, 1);
    t2[0] = t[0];
  } else if ((std::floor(t[0]) == t[0]) && (std::floor(d) == d)) {
    t2.set_size(
        1, static_cast<int>((t[t.size(0) * (t.size(1) - 1)] - t[0]) / d) + 1);
    lastBlockLength = static_cast<int>((t[t.size(1) - 1] - t[0]) / d);
    for (ib = 0; ib <= lastBlockLength; ib++) {
      t2[ib] = t[0] + d * static_cast<double>(ib);
    }
  } else {
    double apnd;
    ndbl = std::floor((t[t.size(1) - 1] - t[0]) / d + 0.5);
    apnd = t[0] + ndbl * d;
    if (d > 0.0) {
      cdiff = apnd - t[t.size(1) - 1];
    } else {
      cdiff = t[t.size(1) - 1] - apnd;
    }
    if (std::abs(cdiff) <
        4.4408920985006262E-16 *
            std::fmax(std::abs(t[0]), std::abs(t[t.size(1) - 1]))) {
      ndbl++;
      apnd = t[t.size(1) - 1];
    } else if (cdiff > 0.0) {
      apnd = t[0] + (ndbl - 1.0) * d;
    } else {
      ndbl++;
    }
    if (ndbl >= 0.0) {
      nx = static_cast<int>(ndbl);
    } else {
      nx = 0;
    }
    t2.set_size(1, nx);
    if (nx > 0) {
      t2[0] = t[0];
      if (nx > 1) {
        t2[nx - 1] = apnd;
        nm1d2 = (nx - 1) / 2;
        for (k = 0; k <= nm1d2 - 2; k++) {
          cdiff = (static_cast<double>(k) + 1.0) * d;
          t2[k + 1] = t[0] + cdiff;
          t2[(nx - k) - 2] = apnd - cdiff;
        }
        if (nm1d2 << 1 == nx - 1) {
          t2[nm1d2] = (t[0] + apnd) / 2.0;
        } else {
          cdiff = static_cast<double>(nm1d2) * d;
          t2[nm1d2] = t[0] + cdiff;
          t2[nm1d2 + 1] = apnd - cdiff;
        }
      }
    }
  }
  // time values for interp.
  b_y.set_size(1, y.size(1));
  lastBlockLength = y.size(1);
  for (ib = 0; ib < lastBlockLength; ib++) {
    b_y[ib] = y[ib];
  }
  x.set_size(1, t.size(1));
  lastBlockLength = t.size(1);
  for (ib = 0; ib < lastBlockLength; ib++) {
    x[ib] = t[ib];
  }
  nx = t.size(1) - 1;
  Vq.set_size(1, t2.size(1));
  lastBlockLength = t2.size(1);
  for (ib = 0; ib < lastBlockLength; ib++) {
    Vq[ib] = 0.0;
  }
  b = (t2.size(1) == 0);
  if (!b) {
    k = 0;
    int exitg1;
    do {
      exitg1 = 0;
      if (k <= nx) {
        if (std::isnan(t[k])) {
          exitg1 = 1;
        } else {
          k++;
        }
      } else {
        if (t[1] < t[0]) {
          ib = (nx + 1) >> 1;
          for (hi = 0; hi < ib; hi++) {
            cdiff = x[hi];
            nm1d2 = nx - hi;
            x[hi] = x[nm1d2];
            x[nm1d2] = cdiff;
          }
          nx = y.size(1) >> 1;
          for (hi = 0; hi < nx; hi++) {
            nm1d2 = (y.size(1) - hi) - 1;
            cdiff = b_y[hi];
            b_y[hi] = b_y[nm1d2];
            b_y[nm1d2] = cdiff;
          }
        }
        coder::interp1SplineOrPCHIP(b_y, t2, Vq, x);
        exitg1 = 1;
      }
    } while (exitg1 == 0);
  }
  c_y.set_size(Vq.size(1));
  lastBlockLength = Vq.size(1);
  for (ib = 0; ib < lastBlockLength; ib++) {
    c_y[ib] = Vq[ib];
  }
  // cubic spline interpolation
  if (c_y.size(0) == 0) {
    cdiff = 0.0;
  } else {
    if (c_y.size(0) <= 1024) {
      nm1d2 = c_y.size(0);
      lastBlockLength = 0;
      nx = 1;
    } else {
      nm1d2 = 1024;
      nx = c_y.size(0) / 1024;
      lastBlockLength = c_y.size(0) - (nx << 10);
      if (lastBlockLength > 0) {
        nx++;
      } else {
        lastBlockLength = 1024;
      }
    }
    cdiff = c_y[0];
    for (k = 2; k <= nm1d2; k++) {
      cdiff += c_y[k - 1];
    }
    for (ib = 2; ib <= nx; ib++) {
      nm1d2 = (ib - 1) << 10;
      ndbl = c_y[nm1d2];
      if (ib == nx) {
        hi = lastBlockLength;
      } else {
        hi = 1024;
      }
      for (k = 2; k <= hi; k++) {
        ndbl += c_y[(nm1d2 + k) - 1];
      }
      cdiff += ndbl;
    }
  }
  cdiff /= static_cast<double>(c_y.size(0));
  lastBlockLength = c_y.size(0);
  for (ib = 0; ib < lastBlockLength; ib++) {
    c_y[ib] = c_y[ib] - cdiff;
  }
  // remove mean
  nm1d2 = 1;
  w_data = 0;
  nx = 0;
  hi = c_y.size(0);
  if (c_y.size(0) == 0) {
    nm1d2 = 0;
    nx = 1;
  } else if (c_y.size(0) == 1) {
    w_data = 1;
    nx = 1;
  }
  d_y.set_size(nm1d2);
  for (ib = 0; ib < nm1d2; ib++) {
    d_y[0] = w_data;
  }
  if (nx == 0) {
    if (c_y.size(0) == 0) {
      d = 0.0;
    } else {
      d = std::fmod(static_cast<double>(c_y.size(0)), 2.0);
    }
    if (d == 0.0) {
      cdiff = static_cast<double>(c_y.size(0)) / 2.0;
      if (cdiff - 1.0 < 0.0) {
        b_y.set_size(1, 0);
      } else {
        b_y.set_size(1, static_cast<int>(cdiff - 1.0) + 1);
        lastBlockLength = static_cast<int>(cdiff - 1.0);
        for (ib = 0; ib <= lastBlockLength; ib++) {
          b_y[ib] = ib;
        }
      }
      d_y.set_size(b_y.size(1));
      lastBlockLength = b_y.size(1);
      for (ib = 0; ib < lastBlockLength; ib++) {
        d_y[ib] =
            6.2831853071795862 * (b_y[ib] / (static_cast<double>(hi) - 1.0));
      }
      nx = d_y.size(0);
      for (k = 0; k < nx; k++) {
        d_y[k] = std::cos(d_y[k]);
      }
      lastBlockLength = d_y.size(0);
      for (ib = 0; ib < lastBlockLength; ib++) {
        d_y[ib] = 0.54 - 0.46 * d_y[ib];
      }
      if (d_y.size(0) < 1) {
        ib = 0;
        nm1d2 = 1;
        hi = -1;
      } else {
        ib = d_y.size(0) - 1;
        nm1d2 = -1;
        hi = 0;
      }
      lastBlockLength = div_s32(hi - ib, nm1d2);
      e_y.set_size((d_y.size(0) + lastBlockLength) + 1);
      nx = d_y.size(0);
      for (hi = 0; hi < nx; hi++) {
        e_y[hi] = d_y[hi];
      }
      for (hi = 0; hi <= lastBlockLength; hi++) {
        e_y[hi + d_y.size(0)] = d_y[ib + nm1d2 * hi];
      }
      d_y.set_size(e_y.size(0));
      lastBlockLength = e_y.size(0);
      for (ib = 0; ib < lastBlockLength; ib++) {
        d_y[ib] = e_y[ib];
      }
    } else {
      cdiff = (static_cast<double>(c_y.size(0)) + 1.0) / 2.0;
      if (cdiff - 1.0 < 0.0) {
        b_y.set_size(1, 0);
      } else {
        b_y.set_size(1, static_cast<int>(cdiff - 1.0) + 1);
        lastBlockLength = static_cast<int>(cdiff - 1.0);
        for (ib = 0; ib <= lastBlockLength; ib++) {
          b_y[ib] = ib;
        }
      }
      d_y.set_size(b_y.size(1));
      lastBlockLength = b_y.size(1);
      for (ib = 0; ib < lastBlockLength; ib++) {
        d_y[ib] =
            6.2831853071795862 * (b_y[ib] / (static_cast<double>(hi) - 1.0));
      }
      nx = d_y.size(0);
      for (k = 0; k < nx; k++) {
        d_y[k] = std::cos(d_y[k]);
      }
      lastBlockLength = d_y.size(0);
      for (ib = 0; ib < lastBlockLength; ib++) {
        d_y[ib] = 0.54 - 0.46 * d_y[ib];
      }
      if (d_y.size(0) - 1 < 1) {
        ib = 0;
        nm1d2 = 1;
        hi = -1;
      } else {
        ib = d_y.size(0) - 2;
        nm1d2 = -1;
        hi = 0;
      }
      lastBlockLength = div_s32(hi - ib, nm1d2);
      e_y.set_size((d_y.size(0) + lastBlockLength) + 1);
      nx = d_y.size(0);
      for (hi = 0; hi < nx; hi++) {
        e_y[hi] = d_y[hi];
      }
      for (hi = 0; hi <= lastBlockLength; hi++) {
        e_y[hi + d_y.size(0)] = d_y[ib + nm1d2 * hi];
      }
      d_y.set_size(e_y.size(0));
      lastBlockLength = e_y.size(0);
      for (ib = 0; ib < lastBlockLength; ib++) {
        d_y[ib] = e_y[ib];
      }
    }
  }
  if (c_y.size(0) == d_y.size(0)) {
    lastBlockLength = c_y.size(0);
    for (ib = 0; ib < lastBlockLength; ib++) {
      c_y[ib] = c_y[ib] * d_y[ib];
    }
  } else {
    times(c_y, d_y);
  }
  // hamming window
  // Calculate PSD
  // Method 1
  //      [A, variance] = arburg(y,AR_order); %AR using Burg method
  //      [H,F] = freqz(1,A,nfft,fs);
  //      PSD=(abs(H).^2).*(variance/fs); %malik, p.67
  // Method 2
  coder::pburg(c_y, AR_order, nfft * 2.0 - 1.0, fs, PSD, F);
  // Method 3
  //       h=spectrum.burg;
  //       hpsd = psd(h, y, 'NFFT', nfft, 'Fs', 2);
  //       F=hpsd.Frequencies;
  //       PSD=hpsd.Data;
}

//
// File trailer for calcAR.cpp
//
// [EOF]
//
